package ru.spbau.lifeincube.gui3d;

import javafx.geometry.Point3D;
import ru.spbau.lifeincube.math.Matrix3x3;
import ru.spbau.lifeincube.math.Quaternion;

public class ArcBall {
    private boolean isDragging = false;
    private int width;
    private int height;
    private Point3D vecNow, vecDown;
    private Quaternion quatNow, quatDown;

    public ArcBall(int width, int height) {
        this.width = width;
        this.height = height;
        quatNow = new Quaternion();
        quatDown = new Quaternion();
    }

    public void begin(double x, double y) {
        isDragging = true;
        vecDown = screenToVector(x, y);
	    System.out.println(String.format("Width : %d", width));
	    System.out.println(String.format("Hegith : %d", height));
	    quatNow = new Quaternion();
	    quatDown = new Quaternion();
//        quatDown = quatNow;
    }

    public void drag(double x, double y) {
        if (isDragging) {
            vecNow = screenToVector(x, y);

            Point3D p = cross(vecDown, vecNow);
//	        System.out.println(String.format("VecDown : (%f, %f, %f)", vecDown.getX(), vecDown.getY(), vecDown.getZ()));
//	        System.out.println(String.format("VecNow : (%f, %f, %f)", vecNow.getX(), vecNow.getY(), vecNow.getZ()));
//	        System.out.println(String.format("Cross : (%f, %f, %f)", p.getX(), p.getY(), p.getZ()));

            if (length(p) > 1e-5) {
	            Quaternion q = new Quaternion(new double[]{p.getX(), p.getY(), p.getZ(), dot(vecDown, vecNow)});
	            q.normalize();
                quatNow = cross(q, quatDown);
            } else {
                quatNow = cross(new Quaternion(), quatDown);
            }
        }
    }

    public Quaternion getQuatNow() {
        if (quatNow.getW() > 1) {
	        quatNow.normalize();
            return quatNow;
        }
        return quatNow;
    }

	public Matrix3x3 getRotationMatrix() {
		return new Matrix3x3(quatNow);
	}

    public void setWindowSize(int width, int height) {
        this.width = width;
        this.height = height;
    }

    public void end() {
        isDragging = false;
    }

    private Point3D screenToVector(double screenX, double screenY) {
	    double x = ((screenX / ((width - 1) / 2)) - 1);
        double y = -((screenY / ((height - 1) / 2)) - 1);

        double len = Math.sqrt(x * x + y * y);
        if (len > 1.0) {
            return new Point3D(x / Math.sqrt(len), y / Math.sqrt(len), 0);
        }

        return new Point3D(x, y, Math.sqrt(1.0 - len));
    }

    private Point3D cross(Point3D vector0, Point3D vector1) {

        double[] temp = {0.0, 0.0, 0.0};

        temp[0] = vector0.getY() * vector1.getZ() - vector0.getZ() * vector1.getY();
        temp[1] = vector0.getZ() * vector1.getX() - vector0.getX() * vector1.getZ();
        temp[2] = vector0.getX() * vector1.getY() - vector0.getY() * vector1.getX();

        return new Point3D(temp[0], temp[1], temp[2]);
    }

    private Quaternion cross(Quaternion q1, Quaternion q2) {
        Quaternion res = new Quaternion();
        res.setW(q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
        res.setX(q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY());
        res.setY(q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ());
        res.setZ(q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX());
        return res;
    }

    private double[] normalize(double[] quat) {
        double len = length(quat);
        if (len <= 1e-5) {
            return new double[]{0.0, 0.0, 0.0, 1.0};
        }
        return new double[]{quat[0] / len, quat[1] / len, quat[2] / len, quat[3] / len};
    }

    private double dot(Point3D vector0, Point3D vector1) {
        return vector0.getX() * vector1.getX() + vector0.getY() * vector1.getY() + vector0.getZ() * vector1.getZ();
    }

    private double length(Point3D p) {
        return Math.sqrt(p.getX() * p.getX() + p.getY() * p.getY() + p.getZ() * p.getZ());
    }

    private double length(double[] p) {
        return Math.sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3]);
    }
}
